/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include "mathutils.h"

void ODEToOGL(const float* R, const float* P, float* M)
{
  M[0]  = R[0]; M[1]  = R[4]; M[2]  = R[8];  M[3]  = 0;
  M[4]  = R[1]; M[5]  = R[5]; M[6]  = R[9];  M[7]  = 0;
  M[8]  = R[2]; M[9]  = R[6]; M[10] = R[10]; M[11] = 0;
  M[12] = P[0]; M[13] = P[1]; M[14] = P[2];  M[15] = 1;
}

//multiply 3x3 matrices M1*M2
void MxM3(const float* M1, const float* M2, float* result)
{
  for (int i=0;i<3;i++)
  {
    for (int j=0;j<3;j++)
    {
      result[i*3+j]=M1[i*3]*M2[j]+M1[i*3+1]*M2[3+j]+M1[i*3+2]*M2[6+j];
    }
  }
}

//multiply ode-style 3x3 matrices (dMatrix3) that actually are 3x4 matrices
void OdeMxM3(const float* M1, const float* M2, float* result)
{
  for (int i=0;i<3;i++)
  {
    for (int j=0;j<3;j++)
    {
      result[i*4+j]=M1[i*4]*M2[j]+M1[i*4+1]*M2[4+j]+M1[i*4+2]*M2[8+j];
    }
  }
}

//multiply 3d-vector V with 3x3 matrix M
void MxV3(const float* M,const float* V, float* result)
{
  result[0]=M[0]*V[0]+M[1]*V[1]+M[2]*V[2];
  result[1]=M[3]*V[0]+M[4]*V[1]+M[5]*V[2];
  result[2]=M[6]*V[0]+M[7]*V[1]+M[8]*V[2]; 
}

void RotateVector(const float* R,const float* v, float* result)
{
  result[0]=R[0]*v[0]+R[1]*v[1]+R[2]*v[2];
  result[1]=R[4]*v[0]+R[5]*v[1]+R[6]*v[2];
  result[2]=R[8]*v[0]+R[9]*v[1]+R[10]*v[2];
}

float DotProduct(const float *v1, const float *v2)
{
  return v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2];
}

//calculates the cross product of two vectors
void CrossProduct(const float *v1, const float *v2, float* result)
{
  result[0]=(v1[1] *v2[2] -v1[2] *v2[1]);
  result[1]=(v1[2] *v2[0] -v1[0] *v2[2]);
  result[2]=(v1[0] *v2[1] -v1[1] *v2[0]);
  
}

float VectorLength(const float *v)
{
  return sqrt(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
}
